/******************************************************************************
**
** File Name:	rtos_kernel_exp.S
**
** Description:	RTOS v2. Implementation of the RTOS kernel.
**
** RTOS ver:	3.0
**
** Modules/Functions:
**
**		void	RTOS_Initialize	( void * )
**		void 	RTOS_StartOS	( void )
**		void	RTOS_EnterISR	( void )
**		void	RTOS_LeaveISR	( void )
**
**		void	_EnterISR		( void )
**		void	_LeaveISR		( void )
**	
** Notes:	Experimental version of the RTOS V2 kernel. See description below 
**			prior to use this services!
** 		
*******************************************************************************
** History:
**
**	10/03/2011 - AK - Separating RTOS list manipulation services into the
**					dedicated library module.
**  10/06/2011 - AK - Enter/Leave code revision/update/support for Xmega
**	07/26/2012 - AK - RTOS API is downgraded to comply with earliest rtos v2
**  08/08/2012 - AK - RTOS source code is checked for byte-by-byte complience 
**					with MPP binary (Dec 8, 2011).
** 		
******************************************************************************/
#include        "rtos.inc"

        _MODULE(rtos_kernel)
		
		_DATASECTION
		
	_GLOBAL(RtosKernelData, RtosKernelDataSz)	
	
        _CODESECTION

;;*****************************************************************************
;; Prototype:	RTOS_API void * RTOS_Initialize(void * )
;; Description:	Initialize RTOS. Actually initialization only sets 
;;				the pointer of system (kernel) stack
;; Passed:		p1h:p1l = Pointer to end of new RTOS kernel stack or NULL
;; Returns:		Pointer to end of RTOS kernel stack
;; Uses:		
;; Calls:		None
;; Assumes:		
;; Notes:		Shuld be called from main function of the application.
;;				Cannot be called after RTOS is started.
;;*****************************************************************************
    _FUNCTION(RTOS_Initialize)

RTOS_Initialize:

    pop     R31									;; Zh => Statck
    pop     R30									;; Zl => Stack
	
    subi    p1l, 0								;; Test p1h:p1l for zero (May not be a high pair in IAR)
    sbci    p1h, 0
	
    brne    rt_i1								;; if p1h:p1l == 0 then load current SP value
	
    in      p1l, _SFR_IO_ADDR(SPL)				;; p1h:p1l = SPH:SPL
    in      p1h, _SFR_IO_ADDR(SPH)
	
rt_i1:

	ldi		Yl,pm_lo8(_IdleTask)
	ldi 	Yh,pm_hi8(_IdleTask)				
	push 	Yl
	push 	Yh

	sbiw	p1l,2								;; adjust stack pointer for short context size
												;; required for idle task
	
    sts     RtosKernelData+RtosStack+NextL, p1l	;; RtosKernelData.RtosStack = new SP value
    sts     RtosKernelData+RtosStack+NextH, p1h
;;    sts     ADDR_LO(KDATA,STACK), p1l			;; RtosKernelData.RtosStack = new SP value
;;    sts     ADDR_HI(KDATA,STACK), p1h
	
    ijmp		

    _ENDFUNC	;; _FUNCTION(RTOS_Initialize)

;;*****************************************************************************
;; Prototype:	RTOS_API void RTOS_EnterISR(void)
;; Description:	Pushes entire register context onto the stack, returning a 
;;				frame pointer to the saved context.  If running in user mode 
;;				upon entry (SysLevel == 0xFF) then switches the stack to the 
;;				kernel and stores the frame pointer in the current 
;;				pRtosTaskID->ContextPointer.
;; Passed:		None
;; Returns:		Y = Frame Pointer (See rtos.inc)
;;				R0 = RtosTaskID.flags
;; Uses:		R14-R18, R26-R32, SysLevel
;; Calls:		None
;; Assumes:		Global interrupt flag is disabled.
;; Notes:		Does not use R16-R25 so passed parameters from C to RTOS
;;              functions can be accessed directly after RTOS_EnterISR() call
;;*****************************************************************************
    _FUNCTION(RTOS_EnterISR)
	_PUBLIC(_EnterISR)

RTOS_EnterISR:

	_ENTER_CRITICAL						;; disable all interrupts 

_EnterISR:
	
		;;
		;; interrupt return address (2 bytes) is on stack - keep it here
		;; call return address (2 bytes) is also on stack - should be replaced 
		;; whith R30:R31 (Zl:Zh) later, after rest of context is saved
		;;		

	;; Interrupts must be disabled here

	push	R29							;; Yh
	push	R28							;; Yl
	push	R27							;; Xh
	push	R26							;; Xl
	push	R25							;; Wh, p1h, r1h

	ldi		Yh, hi8(RtosKernelData)		;; Yl:Yh <- &RtosKernelData
	ldi		Yl, lo8(RtosKernelData)

	in		R25, _SFR_IO_ADDR(SREG)		;; R25 <- SREG (Save flags, whith I flag is clear!!!)

	ldd		Xl, Y+SysLevel				;; Xl <- RtosKernelData.SysLevel
	subi	Xl, lo8(-1)					;; Carry set if results 0
	std		Y+SysLevel, Xl				;; RtosKernelData.SysLevel <- (RtosKernelData.SysLevel + 1)
	
	brcs	SaveFullContext				;; if already in kernel, then save context    

	ldd		Xl, Y+Running+NextL			;; Check if current task = IDLE
	ldd		Xh, Y+Running+NextH			;; Xl:Xh <- RtosKernelData.Running.Next
	or		Xh, Xl						;; Carry cleared if results 0
	
	brne	SaveFullContext				;; if interrupting whatever except IDLE then save context
	
        ;;
        ;; When interrupting IDLE, just reset the stack pointer to PRIOR the interrupt.
        ;; in RTOS_LeaveISR, if still IDLE, control will return to the start of the IDLE loop.
        ;; I.E. we don't need the return address to IDLE, just the return address to the
        ;; caller of Prolog() (the interrupt handler).
        ;;
        ;; The code, below, is obscure, but cycle efficient.  The net effect is to reset
        ;; the stack to RtosStack before returning to the interrupt handler.
        ;;
		;; Grabe frame pointer and adjust it...
	
	in		Yl, _SFR_IO_ADDR(SPL)		;; Yl:Yh <- SPL:SPH (stack pointer)
	in		Yh, _SFR_IO_ADDR(SPH)		
	ldd		Zh, Y+6         			;; Zl:Zh <- SP+(6:7) return-to-__vector__ address ))
	ldd		Zl, Y+7
	adiw	Yl, 7						;; Yl:Yh += 7 - Adjust stack pointer (2 bytes ret to isr + 5 bytes registers)
	out		_SFR_IO_ADDR(SPL), Yl  		;; SPL:SPH <- Yl:Yh
	out		_SFR_IO_ADDR(SPH), Yh		;; This is cycle efficient, but obscure.
	
	_LEAVE_CRITICAL						;; Now we can enable all interrupts 
	
	ijmp								;; ~37 cycles for IDLE task.

SaveFullContext:						;; Save the rest of context aka "full context"

	push	R24							;; Wl, p1l, r1l
    push    R23
    push    R22
    push    R21
    push    R20
    push    R19
    push    R18
    push    R17
    push    R16
    push    R15
    push    R14
    push    R13
    push    R12
    push    R11
    push    R10
    push    R9
    push    R8
    push    R7
    push    R6
    push    R5
    push    R4
    push    R3
    push    R2
    push    R1
    push    R0
	
    push    R25							;; push SREG

	in		Yl, _SFR_IO_ADDR(SPL)		;; Yl:Yh <- SPL:SPH
	in		Yh, _SFR_IO_ADDR(SPH)   	;; Grab frame pointer

	ldd		R25, Y+_R25					;; Need to restore this register (used for SREG)
	ldd		tmp1, Y+_RetLo				;; tmp1:tmp2 <- RetLo:RetHi (return address)
	ldd		tmp2, Y+_RetHi
    std     Y+_R30, Zl					;; SP+offset(R30:R31) <- Zl:Zh
    std     Y+_R31, Zh      			;; Swap return address with Z

	brcs	AlreadyInKernel 			;; Flags, still good, from above.

	ldi		Zh, hi8(RtosKernelData)		;; Zl:Zh <- &RtosKernelData
	ldi		Zl, lo8(RtosKernelData)
	ldd		Yh, Z+Running+NextH    		;; Yl:Yh <- RtosKernelData.Running.Next
	ldd		Yl, Z+Running+NextL			;; Point to running task 
	in		Xh, _SFR_IO_ADDR(SPH)		;; Xl:Xh <- SPL:SPH (stack pointer)
	in		Xl, _SFR_IO_ADDR(SPL)
	std		Y+PidSP+NextH, Xh			;; RtosKernelData.Running.Next.ContextPointer <- Xl:Xh
	std		Y+PidSP+NextL, Xl
										;; Swap to kernel stack
    ldd		tmp0, Z+RtosStack+NextL		;; tmp0 <- RtosKernelData.RtosStack.NextL
	out		_SFR_IO_ADDR(SPL), tmp0		;; SPL  <- RtosKernelData.RtosStack.NextL
	ldd		tmp0, Z+RtosStack+NextH		;; tmp0 <- RtosKernelData.RtosStack.NextH
	out		_SFR_IO_ADDR(SPH), tmp0		;; SPH  <- RtosKernelData.RtosStack.NextH
	mov		Yl, Xl						;; Yl:Yh <- Xl:Xh (previous SPL:SPH)
	mov		Yh, Xh						;; restore frame pointer

AlreadyInKernel:                	

	clr     R1              			;; R1 = __Zero__ for Avr-gcc
    mov     Zl, tmp1        			;; Zl:Zh <- tmp1:tmp2 (RetLo:RetHi)
    mov     Zh, tmp2					
	
	;;_LEAVE_CRITICAL 					;; Now we can allow interrupts of upper levels
	
	ijmp								;; Return to caller (89/106)
	
    _ENDFUNC	;; _EnterISR()

;;*****************************************************************************
;; Prototype:	RTOS_API void RTOS_StartOS(void)
;; Description: Start RTOS scheduler
;; Passed:		
;; Returns:		
;; Uses:		
;; Calls:		
;; Assumes:		
;; Notes:		
;;*****************************************************************************
	_FUNCTION(RTOS_StartOS)

RTOS_StartOS:

	POP		R30							;; Freeing two bytes of stack as we 
	POP		R30							;; don't need return-to-caller (main) address
	
	_ENDFUNC	;; _FUNCTION(RTOS_Start)
	
;;*****************************************************************************
;; Prototype:	RTOS_API void RTOS_LeaveISR(void)
;; Description:	Restore previous context (kernel or user).
;; Passed:		
;; Returns:		
;; Uses:		
;; Calls:		
;; Assumes:		This service assumes that every interrupt vector is defined
;;				as follows:
;;				 	vector_nn:
;;						CLI
;;						CALL	RTOS_EnterISR
;;						CALL	USER_HANDLER_FUNCTION
;;						JMP		RTOS_LeaveISR
;;				This is important to use JMP (but not CALL) to keep stack 
;;				unchanged. It is reccomended to use RTOS_DECLARE_ISR() to 
;;				comply with	this rule. Macro is defined in rtos.h
;; Notes:		
;;*****************************************************************************
    _FUNCTION(RTOS_LeaveISR)
    _PUBLIC(_LeaveISR)

RTOS_LeaveISR:
															
	_ENTER_CRITICAL

_LeaveISR:

	;; Interrupts MUST be disabled here

	ldi		Zl, lo8(RtosKernelData)
	ldi		Zh, hi8(RtosKernelData)
	
    ldd     R16, Z+SysLevel    		;; Load and decrement SysLevel
    dec     R16						;; required for nested interrupt processing
    std		Z+SysLevel, R16			;; 
	
    brge    SkipTaskSwap			;; If SysLevel != 0 then skip Task Swap (nested interrupt)

    ldd     Yh, Z+RunQueue+NextH
    ldd     Yl, Z+RunQueue+NextL
    std     Z+Running+NextH, Yh
    std     Z+Running+NextL, Yl   	;; Update current running task
    adiw    Yl, 0					;; Check if it is IdleTask
	
	breq 	ReturnToIdle			

    ldd     Xh, Y+PidSP+NextH
    ldd     Xl, Y+PidSP+NextL
    out     _SFR_IO_ADDR(SPL), Xl
    out     _SFR_IO_ADDR(SPH), Xh   ;; 20 cycles

SkipTaskSwap:                   	;; 20/6

    pop     R0
    out     _SFR_IO_ADDR(SREG), R0	;; this copy of SREG has I bit clear. 
									;; It should be enabled separately!
    pop     R0
    pop     R1
    pop     R2
    pop     R3
    pop     R4
    pop     R5
    pop     R6
    pop     R7
    pop     R8
    pop     R9
    pop     R10
    pop     R11
    pop     R12
    pop     R13
    pop     R14
    pop     R15
    pop     R16
    pop     R17
    pop     R18
    pop     R19
    pop     R20
    pop     R21
    pop     R22
    pop     R23
    pop     R24
    pop     R25
    pop     R26
    pop     R27
    pop     R28
    pop     R29
    pop     R30
    pop     R31

ReturnToIdle:	

	sei								;; Enable interrupts
	
	reti							;; restore PMIC state (does not affect I bit in SREG!!!)

	_ENDFUNC	;; _FUNCTION(_LeaveISR)
	
;;*****************************************************************************
;; Prototype:	void _IdleTask(void)
;; Description:	RTOS calls this if there are no entries in the _RunQueue. For 
;;				example - no isrs and all the tasks are waiting something, so
;;				scheduling process will continue as soon as some interrupt sets
;;				event.
;; Passed:		None
;; Returns:		None
;; Uses:		None
;; Calls:		None
;; Assumes:		See description
;; Notes:		RTOS Internal Service. Not to use in C-code!!!
;;*****************************************************************************
	_FUNCTION(_IdleTask) 
	
_IdleTask:

;;      sleep                		;; Power Down..
        rjmp    _IdleTask
		
    _ENDFUNC ;; _FUNCTION(_IdleTask)

		_END	 ;; _MODULE(rtos_kernel)

	